/****************************************************************************
 *   $Id:: blinky_main.c 4785 2010-09-03 22:39:27Z nxp21346                        $
 *   Project: LED flashing / ISP test program
 *
 *   Description:
 *     This file contains the main routine for the blinky code sample
 *     which flashes an LED on the LPCXpresso board and also increments an
 *     LED display on the Embedded Artists base board. This project
 *     implements CRP and is useful for testing bootloaders.
 *
 ****************************************************************************/

#include "driver_config.h"
#include "target_config.h"

//#include "timer32.h"		// sys tick tmr
//#include "timer16.h"		// pwm
//#include "gpio.h"			// led
//#include "adc.h"			//
#include "debug_printf.h"	// soft console
#include "application.h"

/******************************************************************************
 * External global variables
 *****************************************************************************/
extern volatile uint32_t SysTick_cntr;
//extern volatile MOTOR_TypeDef Motor;
extern volatile MOTOR_TypeDef Servo;

//volatile uint32_t periodServo = 1000000; //~48Hz PWM frequency, must match max ADC value
//#define SERVO_PERIOD	1000000 			//~48Hz PWM frequency, must match max ADC value
//#define SERVO_MAX		980000		// 10% DC is full CCW
//#define SERVO_MIN		900000				//SERVO_PERIOD * 0.9
/******************************************************************************
 ** Function name: Call_5ms
 **
 ** Descriptions:
 **
 ** parameters:
 ** Returned value:
 **
 ******************************************************************************/
void Call_5ms(void)
{
	//	if (Motor.Enable && (Motor.RampingUp == FALSE))
	//	{
	//		/* Do the PID calculations */
	//		vPID_RPM(&Motor);
	//	}

	/* Do the PID calculations */
	Servo.pv = ADCValue[0];
	vPID_RPM(&Servo);
	setServo(1,Servo.mv);

}
/******************************************************************************
 ** Function name:
 **
 ** Descriptions:
 **
 ** parameters:
 ** Returned value:
 **
 ******************************************************************************/
void Call_25ms(void)
{
}
/******************************************************************************
 ** Function name:
 **
 ** Descriptions:
 **
 ** parameters:
 ** Returned value:
 **
 ******************************************************************************/
void Call_100ms(void)
{
	//	if (Motor.Enable && Motor.RampingUp)
	//	{
	//		/* Ramp-up the motor.. */
	//		vBLDC_RampUp (&Motor, Motor.sp);
	//	}

}

/******************************************************************************
 ** Function name:
 **
 ** Descriptions:
 **
 ** parameters:
 ** Returned value:
 **
 ******************************************************************************/
void Call_1s(void)
{
	/*show RPM every second*/
	//	debug_printf("r/ RPM : %d",Motor.RPM);
}
/******************************************************************************
 ** Function name:
 **
 ** Descriptions:
 **
 ** parameters:
 ** Returned value:
 **
 ******************************************************************************/
void Appl_Init(void)
{
	/****************************************************/
	/* SYSTICK 											*/
	/****************************************************/
	SysTick_Config(SysTick_VALUE);

	/****************************************************/
	/* FREERUNNING COUNTER								*/
	/****************************************************/
	/* Setup Timer16 1 as free running counter for e.g. RPM calc..  */
	/* Enable the clock to Timer16 1 */
	LPC_SYSCON->SYSAHBCLKCTRL |= (1 << 8);
	/* Set the prescaler to get never get an overflow */
	LPC_TMR16B1->PR = 55;
	/* RESET timer16 1  */
	LPC_TMR16B1->TCR = 1 << 1;
	/* ENABLE timer16 1 */
	LPC_TMR16B1->TCR = 1;

	/****************************************************/
	/* HALL sensors	INIT								*/
	/****************************************************/
	GPIOInit();
	/* Set the direction to input */
	GPIOSetDir(HALL_A_PORT, HALL_A_PIN, 0); // HALL A

	/* Setup the interrupt, seq: portNum, bitPosi, sense = edge, single = one, event*/
	GPIOSetInterrupt(HALL_A_PORT, HALL_A_PIN, 0, 0, 1);

	/* Enable the interrupts */
	GPIOIntEnable(HALL_A_PORT, HALL_A_PIN); // HALL A

	NVIC_EnableIRQ(EINT2_IRQn);

	/* Initialize GPIO (sets up clock) */
	GPIOInit();
	/* Set LED port pin to output */
	GPIOSetDir(LED_PORT, LED_BIT, 1);

	/* Initialize ADC  */
	ADCInit(ADC_CLK);

	/* PWMs use MR3 for period */

	//TODO moce to servoInit()

//	/* Gas valve PWM */
//	/* Initialize the PWM in timer32_1 enabling match0 output PIO0_1 */
//	init_timer32PWM(0, SERVO_PERIOD, MATCH2);
//	setMatch_timer32PWM(0, 2, SERVO_MIN);
//	//enable_timer32(0);
//
//	/* Throttle PWM */
//	/* Initialize the PWM in timer32_1 enabling match0 output PIO1_7 */
//	init_timer32PWM(0, SERVO_PERIOD, MATCH1);
//	setMatch_timer32PWM(0, 1, SERVO_MIN);
//	enable_timer32(0);

	/* init motor settings */
//	vBLDC_Init(&Motor);
	initServo(&Servo);

	/****************************************************/
	/* I2C INIT      									*/
	/****************************************************/
	//    I2CInit( (uint32_t)I2CMASTER);


	/****************************************************/
	/* KAY INIT   									*/
	/****************************************************/
	//#if MC_BOARD_ENABLE_JOYSTICK == 1
	//	key_init();
	//#endif

}

/*
 * Set value on servo engine
 * range 0-1024
 * */
//void setServo(uint8_t device, uint32_t value)
//{
//	uint32_t dcVal;
//	/*Futabe servo uses 0-10% dutycycle*/
//	dcVal = ((((SERVO_MAX - SERVO_MIN) * value) >> 10) + SERVO_MIN);
//
//	if (device == 1)
//		setMatch_timer32PWM(0, 1, dcVal);
//	else if (device == 2)
//		setMatch_timer32PWM(0, 2, dcVal);
//
//}

/*
 *  Main Program
 **/

int main(void)
{
	int i = 0, on = 0;
	/* Basic chip initialization is taken care of in SystemInit() called
	 * from the startup code. SystemInit() and chip settings are defined
	 * in the CMSIS system_<part family>.c file.
	 */

	/* Initialize 32-bit timer 0. TIME_INTERVAL is defined as 10mS */
	/* You may also want to use the Cortex SysTick timer to do this */
	//init_timer32(0, TIME_INTERVAL);
	/* Enable timer 0. Our interrupt handler will begin incrementing
	 * the TimeTick global each time timer 0 matches and resets.
	 */
	//enable_timer32(0);
	Appl_Init();

	/* welcomme banner, PRINT ON LPCEXPRESSO CONSOLE */
	debug_printf("\n\r/**********************************************/");
	debug_printf("\n\r/*                                            */");
	debug_printf("\n\r/* PID Demo Application                       */");
	debug_printf("\n\r/* MOJ May 7 2011, Ver .1                     */");
	debug_printf("\n\r/**********************************************/\n");

	while (1) /* Loop forever */
	{
		/* tick counters */
		uint32_t cnt_5ms = 0, cnt_25ms = 0, cnt_100ms = 0, cnt_1s = 0;

		/* schedule timed tasks */
		if (SysTick_cntr >= cnt_5ms)
		{
			cnt_5ms = SysTick_cntr + 10;
			Call_5ms();
		}
		if (SysTick_cntr >= cnt_25ms)
		{
			cnt_25ms = SysTick_cntr + 25;
			Call_25ms();
		}
		if (SysTick_cntr >= cnt_100ms)
		{
			cnt_100ms = SysTick_cntr + 100;
			Call_100ms();
		}
		if (SysTick_cntr >= cnt_1s)
		{
			cnt_1s = SysTick_cntr + 1000;
			Call_1s();
		}

		/* misc init TODO */
		GPIOSetValue(LED_PORT, LED_BIT, LED_OFF );

		/* ADC value is 10 bit*/
		ADCValue[1] = ADCRead(1);
		ADCValue[3] = ADCRead(3);

		//ADC to is the ref
		ADCValue[2] = ADCRead(2);
		Servo.sp= ADCValue[2];

		//ADC0 is the process value, used in 5ms Task
		ADCValue[0] = ADCRead(0);

		/* set Servos w ADC value */
		//setServo(2, ADCValue[2]);
		//setServo(1, ADCValue[1]);

		GPIOSetValue(LED_PORT, LED_BIT, LED_ON );

		/* Go to sleep to save power between timer interrupts */
		//    __WFI();
	}
}

